

#include "stdafx.h"
#include "Point.h"

#include "Header.h"

	namespace las
	{
		Point::Point()
		{
			m_pHeader = NULL;
		}

		Point::~Point()
		{

		}	
		
		void Point::SetHeader( Header* header )
		{
			m_pHeader = header;
			m_pData = new unsigned char[m_pHeader->GetPointRecordLength()];
		}

		Header* Point::GetHeader() const
		{
			return m_pHeader;
		}

		void* Point::GetData()
		{
			return m_pData;
		}

		void Point::SetData( void* pData )
		{
			memcpy_s(m_pData, m_pHeader->GetPointRecordLength(), pData, m_pHeader->GetPointRecordLength());
		}

		double Point::GetX() const
		{
			std::int32_t raw = GetRawX();
			double output = (raw * m_pHeader->GetScaleX()) + m_pHeader->GetOffsetX();
			return output;
		}

		double Point::GetY() const
		{
			std::int32_t raw = GetRawY();
			double output = (raw * m_pHeader->GetScaleY()) + m_pHeader->GetOffsetY();
			return output;
		}

		double Point::GetZ() const
		{
			std::int32_t raw = GetRawZ();
			double output = (raw * m_pHeader->GetScaleZ()) + m_pHeader->GetOffsetZ();
			return output;
		}

		std::int32_t Point::GetRawX() const
		{
			std::uint8_t pos = 0;
			std::int32_t* output = (std::int32_t*)(m_pData + pos);
			return *output;
		}

		std::int32_t Point::GetRawY() const
		{
			std::uint8_t pos = 4;
			std::int32_t* output = (std::int32_t*)(m_pData + pos);
			return *output;
		}

		std::int32_t Point::GetRawZ() const
		{
			std::uint8_t pos = 8;
			std::int32_t* output = (std::int32_t*)(m_pData + pos);
			return *output;
		}

		void Point::SetCoordinates( double x, double y, double z )
		{
			SetX(x);
			SetY(y);
			SetZ(z);
		}

		void Point::SetX( double value )
		{
			std::int32_t raw;
			value = (value - m_pHeader->GetOffsetX()) / m_pHeader->GetScaleX();
			raw = value>0.0 ? floor(value + 0.5) : raw = ceil(value - 0.5);
			SetRawX(raw);
		}

		void Point::SetY( double value )
		{
			std::int32_t raw;
			value = (value - m_pHeader->GetOffsetY()) / m_pHeader->GetScaleY();
			raw = value>0.0 ? floor(value + 0.5) : raw = ceil(value - 0.5);
			SetRawY(raw);
		}

		void Point::SetZ( double value )
		{
			std::int32_t raw;
			value = (value - m_pHeader->GetOffsetZ()) / m_pHeader->GetScaleZ();
			raw = value>0.0 ? floor(value + 0.5) : raw = ceil(value - 0.5);
			SetRawZ(raw);
		}

		void Point::SetRawX( std::int32_t value )
		{
			std::uint8_t pos = 0;
			*((std::int32_t*)(m_pData+pos)) = value;
		}

		void Point::SetRawY( std::int32_t value )
		{
			std::uint8_t pos = 4;
			*((std::int32_t*)(m_pData+pos)) = value;
		}

		void Point::SetRawZ( std::int32_t value )
		{
			std::uint8_t pos = 8;
			*((std::int32_t*)(m_pData+pos)) = value;
		}

		std::uint16_t Point::GetIntensity() const
		{
			std::uint8_t pos = 12;
			std::uint16_t *output = (std::uint16_t*)(m_pData + pos);
			return *output;
		}

		void Point::SetIntensity( std::uint16_t intensity )
		{
			std::uint8_t pos = 12;
			*((std::uint16_t*)(m_pData+pos)) = intensity;
		}

		std::uint16_t Point::GetReturnNumber() const
		{
			std::uint8_t pos = 14;
			std::uint8_t flags = m_pData[pos];
			std::uint8_t mask;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0x07;
			}
			else
			{
				mask = 0x0F;
			}
			return (flags&mask);
		}

		void Point::SetReturnNumber( std::uint16_t num )
		{
			std::uint8_t pos = 14;
			std::uint8_t flags = m_pData[pos];
			uint8_t mask;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0x07;
			}
			else
			{
				mask = 0x0F;
			}
			flags &= ~mask;
			flags |= mask & (uint8_t)num;
			m_pData[pos] = flags;
		}

		std::uint16_t Point::GetNumberOfReturns() const
		{
			std::uint8_t pos = 14;
			std::uint8_t flags = m_pData[pos];
			std::uint8_t mask;
			std::uint8_t offset;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0x07;
				offset = 3;
			}
			else
			{
				mask = 0x0F;
				offset = 4;
			}
			return ((flags>>offset)&mask);
		}

		void Point::SetNumberOfReturns( std::uint16_t num )
		{
			std::uint8_t pos = 14;
			std::uint8_t flags = m_pData[pos];
			std::uint8_t mask;
			std::uint8_t offset;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0x38;
				offset = 3;
			}
			else
			{
				mask = 0xF0;
				offset = 4;
			}
			flags &= ~mask;
			flags |= mask & ((uint8_t)num<<offset);
			m_pData[pos] = flags;
		}

		std::uint16_t Point::GetClassification() const
		{
			std::uint8_t pos = 15;
			std::uint8_t flags = m_pData[pos];
			std::uint8_t mask;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0xFF;
			}
			else
			{
				mask = 0x0F;
			}
			return (flags&mask);
		}

		void Point::SetClassification( std::uint16_t num )
		{
			std::uint8_t pos = 15;
			std::uint8_t flags = m_pData[pos];
			std::uint8_t mask;
			if(m_pHeader->GetPointFormat()<=5)
			{
				mask = 0xFF;
			}
			else
			{
				mask = 0x0F;
			}
			flags &= ~mask;
			flags |= mask & ((uint8_t)num);
			m_pData[pos] = flags;
		}

		Color Point::GetColor() const
		{
			Color color;
			std::uint8_t f = m_pHeader->GetPointFormat();

			if ( f == 0 || f == 1 || f == 4 || f == 6 || f == 9)
			{
				return color;
			}

			std::uint8_t index_pos = 20;

			if (f == 2)
				index_pos = 20;
			else if (f == 3 || f == 5) 
				index_pos = 28;
			else if (f == 7)
				index_pos = 30;
			else if (f == 8 || f == 10)//has NIR channel
				index_pos = 30;

			std::uint8_t red_pos = index_pos;
			std::uint8_t green_pos = index_pos + 2;
			std::uint8_t blue_pos = index_pos + 4;
			std::uint8_t NIR_pos = index_pos + 6;

			std::uint16_t* p_red = (std::uint16_t*)(m_pData + red_pos);
			std::uint16_t* p_green = (std::uint16_t*)(m_pData + green_pos);
			std::uint16_t* p_blue = (std::uint16_t*)(m_pData + blue_pos);
			std::uint16_t* p_NIR = (std::uint16_t*)(m_pData + NIR_pos);

			color.SetRed(*p_red);
			color.SetGreen(*p_green);
			color.SetBlue(*p_blue);
			if (f == 8 || f == 10)
			{
				color.SetNIR(*p_NIR);
			}
			return color;
		}

		bool Point::SetColor( Color const& value )
		{
			std::uint8_t f = m_pHeader->GetPointFormat();

			if ( f == 0 || f == 1 || f == 4 || f == 6 || f == 9)
			{
				return false;
			}

			std::uint8_t index_pos = 20;

			if (f == 2)
				index_pos = 20;
			else if (f == 3 || f == 5) 
				index_pos = 28;
			else if (f == 7)
				index_pos = 30;
			else if (f == 8 || f == 10)//has NIR channel
				index_pos = 30;
			
			std::uint8_t red_pos = index_pos;
			std::uint8_t green_pos = index_pos + 2;
			std::uint8_t blue_pos = index_pos + 4;
			std::uint8_t NIR_pos = index_pos + 6;

			*((uint16_t*)(m_pData + red_pos)) = value.GetRed();
			*((uint16_t*)(m_pData + green_pos)) = value.GetGreen();
			*((uint16_t*)(m_pData + blue_pos)) = value.GetBlue();
			if (f == 8 || f == 10)
			{
				*((uint16_t*)(m_pData + NIR_pos)) = value.GetNIR();
			}

			return true;
		}
	}

